% 假设AIS数据存储在一个结构体数组中，每个元素包含Time, Lon, Lat, Course, Speed
clc
clear
currentDir = pwd; % 获取当前目录路径
parentDir = fileparts(currentDir); % 获取当前目录的上一级目录路径
% grandParentDir = fileparts(parentDir); % 获取当前目录的上两级目录路径
% greatGrandParentDir = fileparts(grandParentDir); % 获取当前目录的上三级目录路径

%指定需要查找的文件夹
subdir = 'DATA';
%指定需要查找的文件名
filename = '413168000(NoRepeated).mat';%413168000.mat'; %matlab中包含的是剔除
%构造需要查找的文件的完整路径
full_path = fullfile(parentDir, subdir, filename);

load( full_path)  %此处使用的数据是原始org数据经过后面的[]处理后得到的数据

%% 数据预处理
data(data.Speed<1,:)=[];%锚泊状态不处理
aisData=data;


% 定义异常点检测的阈值
v_speed=15;%船舶设计速度
ship_length=188;
max_speed = 12; % 最大速度阈值，单位节
max_acceleration = v_speed^2/(2*10*ship_length);% 0.02; % 最大加速度，单位节/秒^2，根据实际情况调整
max_distance = 1000; % 最大距离阈值，单位米
max_turn_rate =0.01; % 最大转向率阈值，单位度/秒

% 初始化异常点标记数组
is_anomaly = false(size(aisData,1), 1);

% 检测异常停止点
for i = 2:size(aisData,1)
    if aisData.Speed(i) > max_speed && ...
            abs(aisData.Lon(i) - aisData.Lon(i-1)) < 1e-5 && ...
            abs(aisData.Lat(i) - aisData.Lat(i-1)) < 1e-5 && ...
            abs(aisData.Speed(i) - aisData.Speed(i-1)) < 1e-5 && ...
            abs(aisData.Course(i) - aisData.Course(i-1)) < 1e-5
        is_anomaly(i) = true;
    end
end

% 检测异常加速点
for i = 2:size(aisData,1)
    acceleration = (aisData.Speed(i) - aisData.Speed(i-1)) / (aisData.time(i) - aisData.time(i-1));
    if abs(acceleration) > max_acceleration
        is_anomaly(i) = true;
    end
end

% 检测异常漂移点
for i = 2:size(aisData,1)
    point1=[aisData.Lon(i), aisData.Lat(i)];
    point2=[aisData.Lon(i-1),aisData.Lat(i-1)];
    distance=calculate_distance_two_points(point1,point2);
    thold_distance = aisData.Speed(i-1)*(aisData.time(i)-aisData.time(i-1))*0.514444*1.2;%距离阈值为米
    if distance > thold_distance
        is_anomaly(i) = true;
    end
end

% 检测异常转向点
k=2;
for i=2:size(aisData,1)
    point1=[aisData.Lon(i), aisData.Lat(i)];
    point2=[aisData.Lon(i-1),aisData.Lat(i-1)];
    thold(i,1)=360/(3.14*k*ship_length)* calculate_distance_two_points(point1,point2);
end
thold_mean=mean(thold);

for i = 2:size(aisData,1)
    turn_rate = (aisData.Course(i) - aisData.Course(i-1));% / (aisData.time(i) - aisData.time(i-1));
    point1=[aisData.Lon(i), aisData.Lat(i)];
    point2=[aisData.Lon(i-1),aisData.Lat(i-1)];
    thold=360/(3.14*k*ship_length)* calculate_distance_two_points(point1,point2);
    if abs(turn_rate) > thold %thold_mean
        is_anomaly(i) = true;
    end
end

zlx.ais_deleted=aisData(is_anomaly==0,:);
zlx.degrees_org = calculate_track_degree([aisData.Lon aisData.Lat]);
zlx.degrees_deleted = calculate_track_degree([zlx.ais_deleted.Lon zlx.ais_deleted.Lat]);
zlx.std_org=std(zlx.degrees_org);
zlx.std_deleted = std(zlx.degrees_deleted);


% 初始化插值后的AIS数据table
interpolated_aisData = aisData;

% 找到异常点的索引
anomaly_indices = find(is_anomaly);

% 遍历所有AIS数据点，对异常点进行插值
for i = 1:height(aisData)
    if is_anomaly(i)
        % 找到异常点前后180秒内的非异常点
        start_idx = find(aisData.time <= aisData.time(i) - 180, 1, 'last');
        if isempty(start_idx) || start_idx <= 1
            start_idx = 1; % 如果没有找到前180秒内的点，则从开始
        end
        end_idx = find(aisData.time >= aisData.time(i) + 180, 1, 'first');
        if isempty(end_idx) || end_idx >= height(aisData)
            end_idx = height(aisData); % 如果没有找到后180秒内的点，则到最后
        end
        
        % 获取异常点前后的非异常点数据
        segment = aisData(start_idx:end_idx, {'time', 'Lon', 'Lat', 'Speed', 'Course'});
        
        % 过滤掉异常点
        segment_is_anomaly = is_anomaly(start_idx:end_idx);
        segment = segment(~segment_is_anomaly, :);
        
        % 如果segment包含超过两个点，则进行三次样条插值
        if height(segment) > 1
            % 时间插值
            t = segment.time;
            tq = aisData.time(i); % 插值点为异常点的时间
            
            % 三次样条插值
            interpolated_aisData.Lon(i) = spline(t, segment.Lon, tq);
            interpolated_aisData.Lat(i)  = spline(t, segment.Lat, tq);
            interpolated_aisData.Speed(i)  = spline(t, segment.Speed, tq);
            interpolated_aisData.Course(i) = spline(t, segment.Course, tq);        
        end
    end
end


zlx.ais_spline=interpolated_aisData;
zlx.degrees_spline=calculate_track_degree([interpolated_aisData.Lon interpolated_aisData.Lat]);
zlx.mean_spline=mean(zlx.degrees_spline);


figure
plot(zlx.ais_deleted.Lon,zlx.ais_deleted.Lat,'k^-');
hold on
plot(interpolated_aisData.Lon,interpolated_aisData.Lat,'ro-');



